#include "Matrix4.h"
#include "Vector2.h"
#include "Vector3.h"
#include "Matrix2.h"
#include "Matrix3.h"
#include "Math.h"
#include <string.h>

namespace Core
{
	rtMatrix4 rtMatrix4::mat4_zero(rtVector4(0, 0, 0, 0), rtVector4(0, 0, 0, 0), rtVector4(0, 0, 0, 0), rtVector4(0, 0, 0, 0));
	rtMatrix4 rtMatrix4::mat4_identity(rtVector4(1, 0, 0, 0), rtVector4(0, 1, 0, 0), rtVector4(0, 0, 1, 0), rtVector4(0, 0, 0, 1));

	inline rtMatrix4::rtMatrix4(void) 
	{
	}

	inline rtMatrix4::rtMatrix4(const rtVector4& x, const rtVector4& y, const rtVector4& z, const rtVector4& w) 
	{
		m_Mat[0] = x;
		m_Mat[1] = y;
		m_Mat[2] = z;
		m_Mat[3] = w;
	}

	inline rtMatrix4::rtMatrix4(const float xx, const float xy, const float xz, const float xw,
		const float yx, const float yy, const float yz, const float yw,
		const float zx, const float zy, const float zz, const float zw,
		const float wx, const float wy, const float wz, const float ww) 
	{
		m_Mat[0][0] = xx; m_Mat[0][1] = xy; m_Mat[0][2] = xz; m_Mat[0][3] = xw;
		m_Mat[1][0] = yx; m_Mat[1][1] = yy; m_Mat[1][2] = yz; m_Mat[1][3] = yw;
		m_Mat[2][0] = zx; m_Mat[2][1] = zy; m_Mat[2][2] = zz; m_Mat[2][3] = zw;
		m_Mat[3][0] = wx; m_Mat[3][1] = wy; m_Mat[3][2] = wz; m_Mat[3][3] = ww;
	}

	inline rtMatrix4::rtMatrix4(const rtMatrix3& rotation, const rtVector3& translation) 
	{
		m_Mat[0][0] = rotation[0][0];
		m_Mat[0][1] = rotation[1][0];
		m_Mat[0][2] = rotation[2][0];
		m_Mat[0][3] = translation[0];
		m_Mat[1][0] = rotation[0][1];
		m_Mat[1][1] = rotation[1][1];
		m_Mat[1][2] = rotation[2][1];
		m_Mat[1][3] = translation[1];
		m_Mat[2][0] = rotation[0][2];
		m_Mat[2][1] = rotation[1][2];
		m_Mat[2][2] = rotation[2][2];
		m_Mat[2][3] = translation[2];
		m_Mat[3][0] = 0.0f;
		m_Mat[3][1] = 0.0f;
		m_Mat[3][2] = 0.0f;
		m_Mat[3][3] = 1.0f;
	}

	inline rtMatrix4::rtMatrix4(const float src[4][4]) 
	{
		memcpy(m_Mat, src, 4 * 4 * sizeof(float));
	}

	inline const rtVector4& rtMatrix4::operator [] (int index) const 
	{
		return m_Mat[index];
	}

	inline rtVector4& rtMatrix4::operator [] (int index) 
	{
		return m_Mat[index];
	}

	inline rtMatrix4 rtMatrix4::operator * (const float a) const 
	{
		return rtMatrix4(m_Mat[0].x * a, m_Mat[0].y * a, m_Mat[0].z * a, m_Mat[0].w * a,
			m_Mat[1].x * a, m_Mat[1].y * a, m_Mat[1].z * a, m_Mat[1].w * a,
			m_Mat[2].x * a, m_Mat[2].y * a, m_Mat[2].z * a, m_Mat[2].w * a,
			m_Mat[3].x * a, m_Mat[3].y * a, m_Mat[3].z * a, m_Mat[3].w * a);
	}

	inline rtVector4 rtMatrix4::operator * (const rtVector4& vec) const 
	{
		return rtVector4(m_Mat[ 0 ].x * vec.x + m_Mat[ 0 ].y * vec.y + m_Mat[ 0 ].z * vec.z + m_Mat[ 0 ].w * vec.w,
			m_Mat[ 1 ].x * vec.x + m_Mat[ 1 ].y * vec.y + m_Mat[ 1 ].z * vec.z + m_Mat[ 1 ].w * vec.w,
			m_Mat[ 2 ].x * vec.x + m_Mat[ 2 ].y * vec.y + m_Mat[ 2 ].z * vec.z + m_Mat[ 2 ].w * vec.w,
			m_Mat[ 3 ].x * vec.x + m_Mat[ 3 ].y * vec.y + m_Mat[ 3 ].z * vec.z + m_Mat[ 3 ].w * vec.w);
	}

	inline rtVector3 rtMatrix4::operator * (const rtVector3& vec) const 
	{
		float s = m_Mat[3].x * vec.x + m_Mat[3].y * vec.y + m_Mat[3].z * vec.z + m_Mat[3].w;
		if (s == 0.0f) 
		{
			return rtVector3(0.0f, 0.0f, 0.0f);
		}
		if (s == 1.0f) 
		{
			return rtVector3(m_Mat[0].x * vec.x + m_Mat[0].y * vec.y + m_Mat[0].z * vec.z + m_Mat[0].w,
				m_Mat[1].x * vec.x + m_Mat[1].y * vec.y + m_Mat[1].z * vec.z + m_Mat[1].w,
				m_Mat[2].x * vec.x + m_Mat[2].y * vec.y + m_Mat[2].z * vec.z + m_Mat[2].w);
		}
		else 
		{
			float invS = 1.0f / s;
			return rtVector3((m_Mat[0].x * vec.x + m_Mat[0].y * vec.y + m_Mat[0].z * vec.z + m_Mat[0].w) * invS,
				(m_Mat[1].x * vec.x + m_Mat[1].y * vec.y + m_Mat[1].z * vec.z + m_Mat[1].w) * invS,
				(m_Mat[2].x * vec.x + m_Mat[2].y * vec.y + m_Mat[2].z * vec.z + m_Mat[2].w) * invS);
		}
	}

	inline rtMatrix4 rtMatrix4::operator * (const rtMatrix4& a) const 
	{
		int i, j;
		const float* m1Ptr, *m2Ptr;
		float* dstPtr;
		rtMatrix4 dst;

		m1Ptr = reinterpret_cast<const float*>(this);
		m2Ptr = reinterpret_cast<const float*>(&a);
		dstPtr = reinterpret_cast<float*>(&dst);

		for (i = 0; i < 4; i++) 
		{
			for (j = 0; j < 4; j++) 
			{
				*dstPtr = m1Ptr[0] * m2Ptr[0 * 4 + j]
				+ m1Ptr[1] * m2Ptr[1 * 4 + j]
				+ m1Ptr[2] * m2Ptr[2 * 4 + j]
				+ m1Ptr[3] * m2Ptr[3 * 4 + j];
				dstPtr++;
			}
			m1Ptr += 4;
		}
		return dst;
	}

	inline rtMatrix4 rtMatrix4::operator + (const rtMatrix4& a) const 
	{
		return rtMatrix4(m_Mat[0].x + a[0].x, m_Mat[0].y + a[0].y, m_Mat[0].z + a[0].z, m_Mat[0].w + a[0].w,
			m_Mat[1].x + a[1].x, m_Mat[1].y + a[1].y, m_Mat[1].z + a[1].z, m_Mat[1].w + a[1].w,
			m_Mat[2].x + a[2].x, m_Mat[2].y + a[2].y, m_Mat[2].z + a[2].z, m_Mat[2].w + a[2].w,
			m_Mat[3].x + a[3].x, m_Mat[3].y + a[3].y, m_Mat[3].z + a[3].z, m_Mat[3].w + a[3].w);
	}

	inline rtMatrix4 rtMatrix4::operator - (const rtMatrix4& a) const 
	{
		return rtMatrix4(m_Mat[0].x - a[0].x, m_Mat[0].y - a[0].y, m_Mat[0].z - a[0].z, m_Mat[0].w - a[0].w,
			m_Mat[1].x - a[1].x, m_Mat[1].y - a[1].y, m_Mat[1].z - a[1].z, m_Mat[1].w - a[1].w,
			m_Mat[2].x - a[2].x, m_Mat[2].y - a[2].y, m_Mat[2].z - a[2].z, m_Mat[2].w - a[2].w,
			m_Mat[3].x - a[3].x, m_Mat[3].y - a[3].y, m_Mat[3].z - a[3].z, m_Mat[3].w - a[3].w);
	}

	inline rtMatrix4 &rtMatrix4::operator *= (const float a) 
	{
		m_Mat[0].x *= a; m_Mat[0].y *= a; m_Mat[0].z *= a; m_Mat[0].w *= a;
		m_Mat[1].x *= a; m_Mat[1].y *= a; m_Mat[1].z *= a; m_Mat[1].w *= a;
		m_Mat[2].x *= a; m_Mat[2].y *= a; m_Mat[2].z *= a; m_Mat[2].w *= a;
		m_Mat[3].x *= a; m_Mat[3].y *= a; m_Mat[3].z *= a; m_Mat[3].w *= a;
		return *this;
	}

	inline rtMatrix4 &rtMatrix4::operator *= (const rtMatrix4& a) 
	{
		*this = (*this) * a;
		return *this;
	}

	inline rtMatrix4& rtMatrix4::operator += (const rtMatrix4& a) 
	{
		m_Mat[0].x += a[0].x; m_Mat[0].y += a[0].y; m_Mat[0].z += a[0].z; m_Mat[0].w += a[0].w;
		m_Mat[1].x += a[1].x; m_Mat[1].y += a[1].y; m_Mat[1].z += a[1].z; m_Mat[1].w += a[1].w;
		m_Mat[2].x += a[2].x; m_Mat[2].y += a[2].y; m_Mat[2].z += a[2].z; m_Mat[2].w += a[2].w;
		m_Mat[3].x += a[3].x; m_Mat[3].y += a[3].y; m_Mat[3].z += a[3].z; m_Mat[3].w += a[3].w;
		return *this;
	}

	inline rtMatrix4& rtMatrix4::operator-=(const rtMatrix4& a) 
	{
		m_Mat[0].x -= a[0].x; m_Mat[0].y -= a[0].y; m_Mat[0].z -= a[0].z; m_Mat[0].w -= a[0].w;
		m_Mat[1].x -= a[1].x; m_Mat[1].y -= a[1].y; m_Mat[1].z -= a[1].z; m_Mat[1].w -= a[1].w;
		m_Mat[2].x -= a[2].x; m_Mat[2].y -= a[2].y; m_Mat[2].z -= a[2].z; m_Mat[2].w -= a[2].w;
		m_Mat[3].x -= a[3].x; m_Mat[3].y -= a[3].y; m_Mat[3].z -= a[3].z; m_Mat[3].w -= a[3].w;
		return *this;
	}

	inline rtMatrix4 operator * (const float a, const rtMatrix4& mat4) 
	{
		return mat4 * a;
	}

	inline rtVector4 operator * (const rtVector4& vec, const rtMatrix4& mat4) 
	{
		return mat4 * vec;
	}

	inline rtVector3 operator * (const rtVector3& vec, const rtMatrix4& mat4) 
	{
		return mat4 * vec;
	}

	inline rtVector4& operator *= (rtVector4& vec, const rtMatrix4& mat4) 
	{
		vec = mat4 * vec;
		return vec;
	}

	inline rtVector3& operator *= (rtVector3& vec, const rtMatrix4& mat4) 
	{
		vec = mat4 * vec;
		return vec;
	}

	inline bool rtMatrix4::compare(const rtMatrix4& a) const 
	{
		size_t i;
		const float* ptr1, *ptr2;

		ptr1 = reinterpret_cast<const float*>(m_Mat);
		ptr2 = reinterpret_cast<const float*>(a.m_Mat);
		for (i = 0; i < 4 * 4; i++) 
		{
			if (ptr1[i] != ptr2[i]) 
			{
				return false;
			}
		}
		return true;
	}

	inline bool rtMatrix4::compare(const rtMatrix4& a, const float epsilon) const 
	{
		size_t i;
		const float* ptr1, *ptr2;

		ptr1 = reinterpret_cast<const float*>(m_Mat);
		ptr2 = reinterpret_cast<const float*>(a.m_Mat);
		for (i = 0; i < 4 * 4; i++) 
		{
			if (rtMath::Fabs(ptr1[i] - ptr2[i]) > epsilon) 
			{
				return false;
			}
		}
		return true;
	}

	inline bool rtMatrix4::operator == (const rtMatrix4& a) const 
	{
		return compare(a);
	}

	inline bool rtMatrix4::operator != (const rtMatrix4& a) const 
	{
		return !compare(a);
	}

	inline void rtMatrix4::zero(void) 
	{
		memset(m_Mat, 0, sizeof(rtMatrix4));
	}

	inline void rtMatrix4::identity(void) 
	{
		*this = mat4_identity;
	}

	inline bool rtMatrix4::isIdentity(const float epsilon) const 
	{
		return compare(mat4_identity, epsilon);
	}

	inline bool rtMatrix4::isSymmetric(const float epsilon) const 
	{
		for (int i = 1; i < 4; i++) 
		{
			for (int j = 0; j < i; j++) 
			{
				if (rtMath::Fabs(m_Mat[i][j] - m_Mat[j][i]) > epsilon) 
				{
					return false;
				}
			}
		}
		return true;
	}

	inline bool rtMatrix4::isDiagonal(const float epsilon) const 
	{
		for (int i = 0; i < 4; i++) 
		{
			for (int j = 0; j < 4; j++) 
			{
				if (i != j && rtMath::Fabs(m_Mat[i][j]) > epsilon) 
				{
					return false;
				}
			}
		}
		return true;
	}

	inline bool rtMatrix4::isRotated(void) const 
	{
		if ( !m_Mat[0][1] && !m_Mat[0][2] &&
			!m_Mat[1][0] && !m_Mat[1][2] &&
			!m_Mat[2][0] && !m_Mat[2][1]) 
		{
			return false;
		}
		return true;
	}

	inline void rtMatrix4::projectVector(const rtVector4& src, rtVector4& dst) const 
	{
		dst.x = src * m_Mat[0];
		dst.y = src * m_Mat[1];
		dst.z = src * m_Mat[2];
		dst.w = src * m_Mat[3];
	}

	inline void rtMatrix4::unprojectVector(const rtVector4& src, rtVector4& dst) const 
	{
		dst = m_Mat[0] * src.x + m_Mat[1] * src.y + m_Mat[2] * src.z + m_Mat[3] * src.w;
	}

	inline float rtMatrix4::trace(void) const 
	{
		return (m_Mat[0][0] + m_Mat[1][1] + m_Mat[2][2] + m_Mat[3][3]);
	}

	inline rtMatrix4 rtMatrix4::inverse(void) const 
	{
		rtMatrix4 invMat;

		invMat = *this;
		int r = invMat.inverseSelf();
		assert(r);
		return invMat;
	}

	inline rtMatrix4 rtMatrix4::inverseFast(void) const 
	{
		rtMatrix4 invMat;

		invMat = *this;
		int r = invMat.inverseFastSelf();
		assert(r);
		return invMat;
	}


	inline int rtMatrix4::getDimension(void) const 
	{
		return 16;
	}

	inline const float* rtMatrix4::toFloatPtr(void) const 
	{
		return m_Mat[0].toFloatPtr();
	}

	inline float* rtMatrix4::toFloatPtr(void) 
	{
		return m_Mat[0].toFloatPtr();
	}

	rtMatrix4 rtMatrix4::transpose(void) const 
	{
		rtMatrix4	transpose;
		int		i, j;

		for(i = 0; i < 4; i++) 
		{
			for(j = 0; j < 4; j++) 
			{
				transpose[i][j] = m_Mat[j][i];
			}
		}
		return transpose;
	}


	rtMatrix4& rtMatrix4::transposeSelf(void) 
	{
		float	temp;
		int		i, j;

		for(i = 0; i < 4; i++) 
		{
			for(j = i + 1; j < 4; j++) 
			{
				temp = m_Mat[i][j];
				m_Mat[i][j] = m_Mat[j][i];
				m_Mat[j][i] = temp;
			}
		}
		return *this;
	}


	float rtMatrix4::determinant(void) const 
	{

		float det2_01_01 = m_Mat[0][0] * m_Mat[1][1] - m_Mat[0][1] * m_Mat[1][0];
		float det2_01_02 = m_Mat[0][0] * m_Mat[1][2] - m_Mat[0][2] * m_Mat[1][0];
		float det2_01_03 = m_Mat[0][0] * m_Mat[1][3] - m_Mat[0][3] * m_Mat[1][0];
		float det2_01_12 = m_Mat[0][1] * m_Mat[1][2] - m_Mat[0][2] * m_Mat[1][1];
		float det2_01_13 = m_Mat[0][1] * m_Mat[1][3] - m_Mat[0][3] * m_Mat[1][1];
		float det2_01_23 = m_Mat[0][2] * m_Mat[1][3] - m_Mat[0][3] * m_Mat[1][2];

		float det3_201_012 = m_Mat[2][0] * det2_01_12 - m_Mat[2][1] * det2_01_02 + m_Mat[2][2] * det2_01_01;
		float det3_201_013 = m_Mat[2][0] * det2_01_13 - m_Mat[2][1] * det2_01_03 + m_Mat[2][3] * det2_01_01;
		float det3_201_023 = m_Mat[2][0] * det2_01_23 - m_Mat[2][2] * det2_01_03 + m_Mat[2][3] * det2_01_02;
		float det3_201_123 = m_Mat[2][1] * det2_01_23 - m_Mat[2][2] * det2_01_13 + m_Mat[2][3] * det2_01_12;

		return (-det3_201_123 * m_Mat[3][0] + det3_201_023 * m_Mat[3][1] - det3_201_013 * m_Mat[3][2] + det3_201_012 * m_Mat[3][3]);
	}


	bool rtMatrix4::inverseSelf( void ) 
	{

		double det, invDet;

		float det2_01_01 = m_Mat[0][0] * m_Mat[1][1] - m_Mat[0][1] * m_Mat[1][0];
		float det2_01_02 = m_Mat[0][0] * m_Mat[1][2] - m_Mat[0][2] * m_Mat[1][0];
		float det2_01_03 = m_Mat[0][0] * m_Mat[1][3] - m_Mat[0][3] * m_Mat[1][0];
		float det2_01_12 = m_Mat[0][1] * m_Mat[1][2] - m_Mat[0][2] * m_Mat[1][1];
		float det2_01_13 = m_Mat[0][1] * m_Mat[1][3] - m_Mat[0][3] * m_Mat[1][1];
		float det2_01_23 = m_Mat[0][2] * m_Mat[1][3] - m_Mat[0][3] * m_Mat[1][2];

		float det3_201_012 = m_Mat[2][0] * det2_01_12 - m_Mat[2][1] * det2_01_02 + m_Mat[2][2] * det2_01_01;
		float det3_201_013 = m_Mat[2][0] * det2_01_13 - m_Mat[2][1] * det2_01_03 + m_Mat[2][3] * det2_01_01;
		float det3_201_023 = m_Mat[2][0] * det2_01_23 - m_Mat[2][2] * det2_01_03 + m_Mat[2][3] * det2_01_02;
		float det3_201_123 = m_Mat[2][1] * det2_01_23 - m_Mat[2][2] * det2_01_13 + m_Mat[2][3] * det2_01_12;

		det = (-det3_201_123 * m_Mat[3][0] + det3_201_023 * m_Mat[3][1] - det3_201_013 * m_Mat[3][2] + det3_201_012 * m_Mat[3][3]);

		if (rtMath::Fabs(det) < MATRIX_INVERSE_EPSILON) 
		{
			return false;
		}

		invDet = 1.0f / det;

		float det2_03_01 = m_Mat[0][0] * m_Mat[3][1] - m_Mat[0][1] * m_Mat[3][0];
		float det2_03_02 = m_Mat[0][0] * m_Mat[3][2] - m_Mat[0][2] * m_Mat[3][0];
		float det2_03_03 = m_Mat[0][0] * m_Mat[3][3] - m_Mat[0][3] * m_Mat[3][0];
		float det2_03_12 = m_Mat[0][1] * m_Mat[3][2] - m_Mat[0][2] * m_Mat[3][1];
		float det2_03_13 = m_Mat[0][1] * m_Mat[3][3] - m_Mat[0][3] * m_Mat[3][1];
		float det2_03_23 = m_Mat[0][2] * m_Mat[3][3] - m_Mat[0][3] * m_Mat[3][2];

		float det2_13_01 = m_Mat[1][0] * m_Mat[3][1] - m_Mat[1][1] * m_Mat[3][0];
		float det2_13_02 = m_Mat[1][0] * m_Mat[3][2] - m_Mat[1][2] * m_Mat[3][0];
		float det2_13_03 = m_Mat[1][0] * m_Mat[3][3] - m_Mat[1][3] * m_Mat[3][0];
		float det2_13_12 = m_Mat[1][1] * m_Mat[3][2] - m_Mat[1][2] * m_Mat[3][1];
		float det2_13_13 = m_Mat[1][1] * m_Mat[3][3] - m_Mat[1][3] * m_Mat[3][1];
		float det2_13_23 = m_Mat[1][2] * m_Mat[3][3] - m_Mat[1][3] * m_Mat[3][2];

		float det3_203_012 = m_Mat[2][0] * det2_03_12 - m_Mat[2][1] * det2_03_02 + m_Mat[2][2] * det2_03_01;
		float det3_203_013 = m_Mat[2][0] * det2_03_13 - m_Mat[2][1] * det2_03_03 + m_Mat[2][3] * det2_03_01;
		float det3_203_023 = m_Mat[2][0] * det2_03_23 - m_Mat[2][2] * det2_03_03 + m_Mat[2][3] * det2_03_02;
		float det3_203_123 = m_Mat[2][1] * det2_03_23 - m_Mat[2][2] * det2_03_13 + m_Mat[2][3] * det2_03_12;

		float det3_213_012 = m_Mat[2][0] * det2_13_12 - m_Mat[2][1] * det2_13_02 + m_Mat[2][2] * det2_13_01;
		float det3_213_013 = m_Mat[2][0] * det2_13_13 - m_Mat[2][1] * det2_13_03 + m_Mat[2][3] * det2_13_01;
		float det3_213_023 = m_Mat[2][0] * det2_13_23 - m_Mat[2][2] * det2_13_03 + m_Mat[2][3] * det2_13_02;
		float det3_213_123 = m_Mat[2][1] * det2_13_23 - m_Mat[2][2] * det2_13_13 + m_Mat[2][3] * det2_13_12;

		float det3_301_012 = m_Mat[3][0] * det2_01_12 - m_Mat[3][1] * det2_01_02 + m_Mat[3][2] * det2_01_01;
		float det3_301_013 = m_Mat[3][0] * det2_01_13 - m_Mat[3][1] * det2_01_03 + m_Mat[3][3] * det2_01_01;
		float det3_301_023 = m_Mat[3][0] * det2_01_23 - m_Mat[3][2] * det2_01_03 + m_Mat[3][3] * det2_01_02;
		float det3_301_123 = m_Mat[3][1] * det2_01_23 - m_Mat[3][2] * det2_01_13 + m_Mat[3][3] * det2_01_12;

		m_Mat[0][0] =	- det3_213_123 * invDet;
		m_Mat[1][0] = + det3_213_023 * invDet;
		m_Mat[2][0] = - det3_213_013 * invDet;
		m_Mat[3][0] = + det3_213_012 * invDet;

		m_Mat[0][1] = + det3_203_123 * invDet;
		m_Mat[1][1] = - det3_203_023 * invDet;
		m_Mat[2][1] = + det3_203_013 * invDet;
		m_Mat[3][1] = - det3_203_012 * invDet;

		m_Mat[0][2] = + det3_301_123 * invDet;
		m_Mat[1][2] = - det3_301_023 * invDet;
		m_Mat[2][2] = + det3_301_013 * invDet;
		m_Mat[3][2] = - det3_301_012 * invDet;

		m_Mat[0][3] = - det3_201_123 * invDet;
		m_Mat[1][3] = + det3_201_023 * invDet;
		m_Mat[2][3] = - det3_201_013 * invDet;
		m_Mat[3][3] = + det3_201_012 * invDet;

		return true;
	}


	bool rtMatrix4::inverseFastSelf(void) 
	{
		rtMatrix2 r0, r1, r2, r3;
		float a, det, invDet;
		float *m_Mat = reinterpret_cast<float *>(this);

		det = m_Mat[0*4+0] * m_Mat[1*4+1] - m_Mat[0*4+1] * m_Mat[1*4+0];

		if (rtMath::Fabs(det) < MATRIX_INVERSE_EPSILON) 
		{
			return false;
		}

		invDet = 1.0f / det;

		r0[0][0] =   m_Mat[1*4+1] * invDet;
		r0[0][1] = - m_Mat[0*4+1] * invDet;
		r0[1][0] = - m_Mat[1*4+0] * invDet;
		r0[1][1] =   m_Mat[0*4+0] * invDet;


		r1[0][0] = r0[0][0] * m_Mat[0*4+2] + r0[0][1] * m_Mat[1*4+2];
		r1[0][1] = r0[0][0] * m_Mat[0*4+3] + r0[0][1] * m_Mat[1*4+3];
		r1[1][0] = r0[1][0] * m_Mat[0*4+2] + r0[1][1] * m_Mat[1*4+2];
		r1[1][1] = r0[1][0] * m_Mat[0*4+3] + r0[1][1] * m_Mat[1*4+3];


		r2[0][0] = m_Mat[2*4+0] * r1[0][0] + m_Mat[2*4+1] * r1[1][0];
		r2[0][1] = m_Mat[2*4+0] * r1[0][1] + m_Mat[2*4+1] * r1[1][1];
		r2[1][0] = m_Mat[3*4+0] * r1[0][0] + m_Mat[3*4+1] * r1[1][0];
		r2[1][1] = m_Mat[3*4+0] * r1[0][1] + m_Mat[3*4+1] * r1[1][1];


		r3[0][0] = r2[0][0] - m_Mat[2*4+2];
		r3[0][1] = r2[0][1] - m_Mat[2*4+3];
		r3[1][0] = r2[1][0] - m_Mat[3*4+2];
		r3[1][1] = r2[1][1] - m_Mat[3*4+3];


		det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];

		if (rtMath::Fabs(det) < MATRIX_INVERSE_EPSILON) 
		{
			return false;
		}

		invDet = 1.0f / det;

		a = r3[0][0];
		r3[0][0] =   r3[1][1] * invDet;
		r3[0][1] = - r3[0][1] * invDet;
		r3[1][0] = - r3[1][0] * invDet;
		r3[1][1] =   a * invDet;

		r2[0][0] = m_Mat[2*4+0] * r0[0][0] + m_Mat[2*4+1] * r0[1][0];
		r2[0][1] = m_Mat[2*4+0] * r0[0][1] + m_Mat[2*4+1] * r0[1][1];
		r2[1][0] = m_Mat[3*4+0] * r0[0][0] + m_Mat[3*4+1] * r0[1][0];
		r2[1][1] = m_Mat[3*4+0] * r0[0][1] + m_Mat[3*4+1] * r0[1][1];

		m_Mat[2*4+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0];
		m_Mat[2*4+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1];
		m_Mat[3*4+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0];
		m_Mat[3*4+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1];

		m_Mat[0*4+0] = r0[0][0] - r1[0][0] * m_Mat[2*4+0] - r1[0][1] * m_Mat[3*4+0];
		m_Mat[0*4+1] = r0[0][1] - r1[0][0] * m_Mat[2*4+1] - r1[0][1] * m_Mat[3*4+1];
		m_Mat[1*4+0] = r0[1][0] - r1[1][0] * m_Mat[2*4+0] - r1[1][1] * m_Mat[3*4+0];
		m_Mat[1*4+1] = r0[1][1] - r1[1][0] * m_Mat[2*4+1] - r1[1][1] * m_Mat[3*4+1];

		m_Mat[0*4+2] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0];
		m_Mat[0*4+3] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1];
		m_Mat[1*4+2] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0];
		m_Mat[1*4+3] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1];

		m_Mat[2*4+2] = -r3[0][0];
		m_Mat[2*4+3] = -r3[0][1];
		m_Mat[3*4+2] = -r3[1][0];
		m_Mat[3*4+3] = -r3[1][1];

		return true;
	}
}

